
#include <assert.h>
#include <autoconf.h>
#include <libut.h>
#include <string.h>

#include "frametable.h"
#include "macros.h"
#include "mapping.h"
#include "task.h"
#include "ut_manager/ut.h"
#include "vmem_layout.h"

#include "debug.h"
#define verbose SOS_DEBUG_SWAPTABLE
#include <sys/debug.h>
#include <sys/panic.h>


#include "swaptable.h"

static swap_table _swap_table;

static int _range_alloc_retype_and_map(seL4_Word vaddr, seL4_Word size);

int sos_swap_table_init(void) {
    int err;
    seL4_Word table_size;

    assert(SWAP_TABLE_START > DMA_VEND);
    assert(SWAP_TABLE_START < FRAME_TABLE_START);

    err = _range_alloc_retype_and_map(SWAP_TABLE_START, SWAP_TABLE_SIZE);
    if (err) {
        dprintf(1, "Failed to reserve range for swap table\n");
        return SOS_SWAP_TABLE_OOM;
    }

    _swap_table = (swap_table) SWAP_TABLE_START;
    dprintf(1, "Swap table is %dB from %p\n", SWAP_TABLE_SIZE, _swap_table);

    memset((void*) _swap_table, 0x00, SWAP_TABLE_SIZE);
    dprintf(1, "First cell: %d\n", (*_swap_table));

    return SOS_SWAP_TABLE_SUCCESS;
}

int sos_swap_alloc(swap_table_entry *ste) {
    swap_table_entry head = *_swap_table;
    seL4_Word next_index;
    seL4_Word next_head;

    dprintf(5, "swap_alloc: head = %lu\n", head);

    if (head == 0) {
        // Adjacent frame is next
        next_index = 1;
    } else if (head <  SOS_SWAP_MAX) {
        // head is the next index
        next_index = head;
    } else {
        return SOS_SWAP_TABLE_EXHAUSTED;
    }

    // Set the new list head
    next_head = *(_swap_table + next_index);
    if (next_head == 0) {
        (*_swap_table) = next_index + 1;
    } else {
        (*_swap_table) = next_head;
    }

    (*ste) = next_index;

    dprintf(5, "swap_alloc: got frame %lu. Next free is %lu\n",
            next_index, *_swap_table);
    return SOS_SWAP_TABLE_SUCCESS;
}

int sos_swap_free(swap_table_entry ste) {
    swap_table_entry head = *_swap_table;
    seL4_Word new_next;

    // Figure out what to store in ste
    if (head == 0) {
        new_next = 1;
    } else {
        new_next = head;
    }

    *(_swap_table + ste) = new_next;
    *(_swap_table) = ste;

    dprintf(5, "swap_free: put frame %d back. After that is %d\n", 
            ste, new_next);

    return SOS_SWAP_TABLE_SUCCESS;
}

// XXX Duplication - This is all copied from the frametable.
/* Retype and map a frame */
static seL4_Word _frame_retype_and_map(seL4_Word paddr, seL4_Word vaddr) {
    int err;
    seL4_Word cap;
    err = cspace_ut_retype_addr(paddr, seL4_ARM_SmallPageObject, PAGE_BITS,
                                cur_cspace, &cap);
    conditional_panic(err, "Failed to retype frame");

    err = map_page(cap, seL4_CapInitThreadPD, vaddr, seL4_AllRights,
                   seL4_ARM_Default_VMAttributes);
    conditional_panic(err, "Failed to map page into SOS");

    return cap;
}

/* Allocate enough pages for size bytes and map them contiguously from vaddr up */
/* (This is only to be used in frame_table_init and leaks frames - beware) */
static int _range_alloc_retype_and_map(seL4_Word vaddr, seL4_Word size) {
    int i;
    seL4_Word cur_vaddr = vaddr;
    seL4_Word num_frames = DIVROUND(size, PAGE_SIZE);
    int ret = FRAME_TABLE_SUCCESS;

    for(i = 0; i < num_frames; i++) {
        seL4_Word paddr = ut_alloc(PAGE_BITS);
        if (paddr) {
            _frame_retype_and_map(paddr, cur_vaddr);
            cur_vaddr += PAGE_SIZE;
        } else {
            ret = FRAME_TABLE_OOM;
            break;
        }
    }

    return ret;
}

